using System;
using System.Text;
using Microsoft.SPOT;

namespace XBee.Communications
{
    public class IMUData : CommunicationData
    {
        #region Members
        internal const int GPSQualityLimit = 3;
        private bool satFixed = false;
        private int numSatellites = 0;
        private int imuH = 0;
        private double roll = 0.0;
        private double pitch = 0.0;
        private double yaw = 0.0;
        private double latitude = 0.0;
        private double altitude = 0.0;
        private double longitude = 0.0;
        #endregion

        #region Properties
        public double Roll
        {
            get { return roll; }
        }

        public double Pitch
        {
            get { return pitch; }
        }

        public double Yaw
        {
            get { return yaw; }
        }

        public double Latitude
        {
            get { return latitude; }
        }

        public double Altitude
        {
            get { return altitude; }
        }

        public double Longitude
        {
            get { return longitude; }
        }

        public double GroundVel
        {
            get { return groundVel; }
        }

        public int ImuH 
        { 
            get {   return imuH;    }
        }

        public bool IsGPSOnSignal
        {
            get { return (this.satFixed && (this.numSatellites > GPSQualityLimit)); }
        }  
        #endregion

        #region Overrided Methods
        public override string Serialize()
        {
            // !!!VER:1.8.1,RLL:-0.01,PCH:-0.02,YAW:1.98,IMUH:253,TOW:0***
            string res = string.Empty;
            res += (char)this.ByteStartDelimiter;
            res += "VER:" + this.Version;
            res += ",RLL:" + roll;
            res += ",PCH:" + pitch;
            res += ",YAW:" + yaw;
            res += ",IMUH:0";
            res += ",TOW:0";
            res += (char)this.ByteFinishDelimiter;
            return res;
        }

        public override ICommunicationData Deserialize(string sentence)
        {
            // !!!VER:1.8.1,RLL:-0.01,PCH:-0.02,YAW:1.98,IMUH:253,TOW:0***
            // !!!VER:1.8.1,RLL:0.00,PCH:0.04,YAW:-0.29,IMUH:253,LAT:0,LON:0,ALT:0,COG:0,SOG:0,FIX:0,SAT:0,TOW:100923938

            IMUData res = new IMUData();
            string tempStr;

            try
            {
                sentence = sentence.Trim();
                sentence = sentence.Substring(sentence.LastIndexOf('!') + 1);
                string[] partialSentences = sentence.Split(',');
                tempStr = partialSentences[1].Split(':')[1];
                res.roll = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[2].Split(':')[1];
                res.pitch = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[3].Split(':')[1];
                res.yaw = tempStr.Substring(0, 1) == "-" ? -Convert.ToDouble(tempStr) : Convert.ToDouble(tempStr);
                tempStr = partialSentences[4].Split(':')[1];
                res.imuH = Convert.ToInt16(tempStr);

                if (partialSentences.Length > 6)
                {
                    tempStr = partialSentences[5].Split(':')[1];
                    res.latitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[6].Split(':')[1];
                    res.longitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[7].Split(':')[1];
                    res.altitude = Convert.ToInt16(tempStr);
                    tempStr = partialSentences[10].Split(':')[1];
                    res.satFixed = Convert.ToInt16(tempStr) == 0 ? false : true;
                    tempStr = partialSentences[11].Split(':')[1];
                    res.numSatellites = Convert.ToInt16(tempStr);
                }
                return res;
            }
            catch
            {
                return null;
            }
        } 
        #endregion

    }
}
